/*
*/

	
.arch armv7a
.global reset_vector

.section ".arm_vector", "ax"
.code 32

.global _exception_vectors
_exception_vectors:

    b       reset_vector            /* reset */
    b       undef_vector            /* Undefined Instruction */
    b       swi_vector              /* Software Interrupt */
    b       pabt_vector             /* Prefetch Abort */
    b       dabt_vector             /* Data Abort */
    b       rsvd_vector             /* reserved, not run to this. */
    adr     pc, irq_vector + 1      /* IRQ : read the AIC */
    b       fiq_vector              /* FIQ */


.code 16
.type irq_vector, %function
irq_vector:

	push  	{r0-r3, r12, lr}

	ldr     r3, = arch_irq_process
	mov     lr, pc
	bx      r3
	
	pop     {r0-r3, r12, lr}
	subs	pc, lr, #4


.align 4
.code 32

undef_vector:
    push    { r0-r12, lr }
    mov     r0, #0x4
    b       _excp_comn_proc

swi_vector:
    push    { r0-r12, lr }
    mov     r0, #0x8
    b       _excp_comn_proc

pabt_vector:
    sub     lr, #4
    push    { r0-r12, lr }
    mov     r0, #0xC
    b       _excp_comn_proc

dabt_vector:
    sub     lr, #8
    push    { r0-r12, lr }
    mov     r0, #0x10
    b       _excp_comn_proc

rsvd_vector:
    push    { r0-r12, lr }
    mov     r0, #0x14
    b       _excp_comn_proc

fiq_vector:
    push    { r0-r12, lr }
    mov     r0, #0x1C
    b       _excp_comn_proc


_excp_comn_proc:
    
    mrs     r4, spsr
    push    { r0, r4 }
    stmdb   sp, { r13, r14 }^
    sub     sp, #0x8
    mov     r1, sp
    ldr     r5, = dbg_exception_process
    blx     r5
    b       .


reset_vector:
    
    /* Interrupt disable */
    cpsid   if

    /* cache_disable */
    mrc     p15, 0, r12, c1, c0, 0  /* read SCTLR */
    bic     r12, #(1 << 12)         /* i-cache disable */
    bic     r12, #(1 << 2 | 1 << 0) /* d-cache, mmu disable */
    mcr     p15, 0, r12, c1, c0, 0  /* write SCTLR */
    
    /* set up the stack */
    ldr     r3, =0x17A000           /* 1k bytes : (0x17A000, 0x17A400) */
    add     r3, #0x400
    cpsid   i, #0x17                /* abort */
    mov     sp, r3

    cpsid   i, #0x1B                /* undefined */
    mov     sp, r3                  /* same as abort stack */

    cpsid   i, #0x13                /* supervisor */
    mov     sp, r3                  /* same as abort stack */

    add     r3, #0x1400             /* 5k bytes : (0x17A400, 0x17B800) */
    cpsid   i, #0x11                /* fiq */
    mov     sp, r3

    cpsid   i, #0x12                /* irq */
    mov     sp, r3                  /* save as fiq stack */

    add     r3, #0x4400             /* 17k bytes : (0x17B800, 0x180000) */
    cpsid   i, #0x1F                /* system, stacks */
    mov     sp, r3

    
    /* init vector table */
.L__vector_setup:
    dsb
    isb
    ldr     r0, =_exception_vectors
    mcr     p15, 0, r0, c12, c0, #0
    dsb
    isb

    /* clear bss */
.L__bss_clear:
    ldr     r0, =__bss_start__
    ldr     r1, =__bss_end__
    mov     r2, #0

.L__bss_loop:
    cmp     r0, r1
    strlt   r2, [r0], #4
    blt     .L__bss_loop

    /* Branch on C code __my_init function */
    movw    r4, #:lower16:__my_init
    movt    r4, #:upper16:__my_init
    blx     r4

    /* Branch on C code Main function (with interworking) */
    movw    r4, #:lower16:main
    movt    r4, #:upper16:main
    blx     r4

    /* dead loop after main return */
__fin_loop:
    nop
    b       __fin_loop



.section ".text", "ax"
.code 32


.type arm_dcc_read, %function
.global arm_dcc_read
arm_dcc_read:
    
1:  mrc     p14, 0, APSR_nzcv, c0, c1, 0
    bne     1b
    mrc     p14, 0, r0, c0, c5, 0
    bx      lr


.type arm_dcc_write, %function
.global arm_dcc_write
arm_dcc_write:
    
1:  mrc     p14, 0, APSR_nzcv, c0, c1, 0
    bcs     1b
    mcr     p14, 0, r0, c0, c5, 0
    bx      lr







